#include "WProgram.h"
#include "WayPoint.h"
WayPoint::WayPoint(float latitude, float longitude)
{
 _latitude=latitude;
 _longitude=longitude;
}

WayPoint::WayPoint()
{

}

float WayPoint::dist(WayPoint wp)
{
 float latPos, longPos, latWay, longWay, delta, x, y;
  float dist = 0;
  
  // Conversion en radians des coordonnées géographiques
  latPos = radians(_latitude);
  longPos = radians(_longitude);
  latWay = radians(wp._latitude);
  longWay = radians(wp._longitude);
  
  delta = longPos - longWay;
  
  dist = 2*asin(sqrt(pow(sin((latPos - latWay)/2),2) + cos(latPos)*cos(latWay)*pow(sin(delta/2),2)));

  dist = 6371030*dist;
  dist = dist;
  
  return dist;
}
